package naito_rescue.router;

import rescuecore2.misc.*;
import rescuecore2.misc.geometry.*;
import rescuecore2.worldmodel.*;
import rescuecore2.standard.entities.*;
import rescuecore2.misc.collections.LazyMap;

import naito_rescue.*;
import naito_rescue.agent.*;
import naito_rescue.router.object.*;
import naito_rescue.object.*;
import java.util.*;

// 幅優先探索
public class SimpleSearchModule extends AbstractRoutingModule
{
	private Map<EntityID, Set<EntityID>> simpleGraph;
	
    public SimpleSearchModule(NAITOAgent owner, ConnectivityGraph graph) {
    	super(owner, graph);
    	
    	StandardWorldModel world = owner.getWorldModel();
        Map<EntityID, Set<EntityID>> neighbours = new LazyMap<EntityID, Set<EntityID>>() {
            @Override
            public Set<EntityID> createValue() {
                return new HashSet<EntityID>();
            }
        };
        for (Entity next : world) {
            if (next instanceof Area) {
                Collection<EntityID> areaNeighbours = ((Area)next).getNeighbours();
                neighbours.get(next.getID()).addAll(areaNeighbours);
            }
        }
        setGraph(neighbours);
    }

    /**
       Set the neighbourhood graph.
       @param newGraph The new neighbourhood graph.
    */
    public void setGraph(Map<EntityID, Set<EntityID>> newGraph) {
        this.simpleGraph = newGraph;
    }

    /**
       Get the neighbourhood graph.
       @return The neighbourhood graph.
    */
    public Map<EntityID, Set<EntityID>> getGraph() {
        return simpleGraph;
    }
	public List<EntityID> search(EntityID start, Collection<EntityID> goals){
        List<EntityID> open = new LinkedList<EntityID>();
        Map<EntityID, EntityID> ancestors = new HashMap<EntityID, EntityID>();
        open.add(start);
        EntityID next = null;
        boolean found = false;
        ancestors.put(start, start);
        do {
            next = open.remove(0);
            if (isGoal(next, goals)) {
                found = true;
                break;
            }
            Collection<EntityID> neighbours = simpleGraph.get(next);
            if (neighbours.isEmpty()) {
                continue;
            }
            for (EntityID neighbour : neighbours) {
                if (isGoal(neighbour, goals)) {
                    ancestors.put(neighbour, next);
                    next = neighbour;
                    found = true;
                    break;
                }
                else {
                    if (!ancestors.containsKey(neighbour)) {
                        open.add(neighbour);
                        ancestors.put(neighbour, next);
                    }
                }
            }
        } while (!found && !open.isEmpty());
        if (!found) {
            // No path
            return null;
        }
        // Walk back from goal to start
        EntityID current = next;
        List<EntityID> path = new LinkedList<EntityID>();
        do {
            path.add(0, current);
            current = ancestors.get(current);
            if (current == null) {
                throw new RuntimeException("Found a node with no ancestor! Something is broken.");
            }
        } while (current != start);
        return path;
	}
}
